#include "Robot.h"

void Robot_init(void){
    //初始化IIC接口
    IoTIoSetFunc(IOT_IO_NAME_10, IOT_IO_FUNC_10_I2C0_SDA);
    IoTIoSetFunc(IOT_IO_NAME_9, IOT_IO_FUNC_9_I2C0_SCL);
    IoTI2cInit(0, 400000);

    pca9685_reset();
    pca9685_init();
}

void Robot_FK_ResetPose(void){
    FK_RUMove(90,135,45,0);
    FK_RBMove(90,135,45,0);
    FK_LUMove(90,135,45,0);
    FK_LBMove(90,135,45,0);
}

void Robot_FK_GetDown(void){
    FK_RUMove(150,135,45,10);
    FK_RBMove(150,135,45,10);
    FK_LUMove(150,135,45,10);
    FK_LBMove(150,135,45,10);
    Robot_PowerOFF();
}

void Robot_IK_ResetPose(void){
    IK_LUMove(-19,25.400,75.66,0);
    IK_LBMove(-19,25.400,75.66,0);
    IK_RBMove(-19,25.400,75.66,0);
    IK_RUMove(-19,25.400,75.66,0);
}

void Robot_Action_0(void){
    IK_LUMove(-19,25.400,50,0);
    IK_LBMove(-19,25.400,50,0);
    IK_RBMove(-19,25.400,50,0);
    IK_RUMove(-19,25.400,50,0);

    hi_udelay(500*1000);

    IK_LUMove(-19,25.400,90,0);
    IK_LBMove(-19,25.400,90,0);
    IK_RBMove(-19,25.400,90,0);
    IK_RUMove(-19,25.400,90,0);

    hi_udelay(500*1000);

    IK_LUMove(-19,25.400,70,0);
    IK_LBMove(-19,25.400,70,0);
    IK_RBMove(-19,25.400,70,0);
    IK_RUMove(-19,25.400,70,0);
}

void Robot_PowerOFF(void){
    for(uint8_t i = 0;i < 16;i++){
        pca9685_unload(i);
    }
}

void Robot_LegUnload(uint8_t legNum){
    switch(legNum){
        case 1: pca9685_unload(1);  pca9685_unload(2);  pca9685_unload(3);  break;   //1--->左前
        case 2: pca9685_unload(6);  pca9685_unload(5);  pca9685_unload(4);  break;   //2--->左后
        case 3: pca9685_unload(9);  pca9685_unload(10); pca9685_unload(11); break;   //3--->右后
        case 4: pca9685_unload(12); pca9685_unload(13); pca9685_unload(14); break;   //4--->右前
    }
}
/* ------------------ 单腿控制 ------------------ */
// ********************************** //
// ************** 正解 ************** //
// ********************************** //
void FK_RBMove(float posk,float posx,float posh,uint16_t Time){
    pca9685_write(9 ,constrain(posk,KServoMin,KServoMax));       hi_udelay(Time*100);   //K
    pca9685_write(10,constrain(posx,TServoMin,TServoMax));       hi_udelay(Time*100);   //X
    pca9685_write(11,constrain(posh,SServoMin,SServoMax));       hi_udelay(Time*100);   //H
}

void FK_RUMove(float posk,float posx,float posh,uint16_t Time){
    pca9685_write(14,constrain(posk,KServoMin,KServoMax));       hi_udelay(Time*100);   //K
    pca9685_write(13,constrain(posx,TServoMin,TServoMax));       hi_udelay(Time*100);   //X
    pca9685_write(12,constrain(posh,SServoMin,SServoMax));       hi_udelay(Time*100);   //H constrain(posh,SServoMin,SServoMax)
}

void FK_LUMove(float posk,float posx,float posh,uint16_t Time){
    pca9685_write(1 ,constrain(posk,KServoMin,KServoMax));       hi_udelay(Time*100);   //K
    pca9685_write(2 ,constrain(posx,TServoMin,TServoMax));       hi_udelay(Time*100);   //X
    pca9685_write(3 ,constrain(posh,SServoMin,SServoMax));       hi_udelay(Time*100);   //H
}

void FK_LBMove(float posk,float posx,float posh,uint16_t Time){
    pca9685_write(6 ,constrain(posk,KServoMin,KServoMax));       hi_udelay(Time*100);   //K
    pca9685_write(5 ,constrain(posx,TServoMin,TServoMax));       hi_udelay(Time*100);   //X
    pca9685_write(4 ,constrain(posh,SServoMin,SServoMax));       hi_udelay(Time*100);   //H
}

uint8_t FK_LegServoDebug(float Angle[],uint8_t cmd,uint8_t legNum){

    uint8_t offset      = 1;

    switch(cmd){
        //K
        case '4': Angle[0] += offset;break;
        case '1': Angle[0] -= offset;break;
        //T
        case '5': Angle[1] += offset;break;
        case '2': Angle[1] -= offset;break;
        //S
        case '6': Angle[2] += offset;break;
        case '3': Angle[2] -= offset;break;
        //Leg
        case '0':
            Robot_LegUnload(legNum);

            legNum++;
            if(legNum == 5){
                legNum = 1;
            }

            Angle[0]    = 90;
            Angle[1]    = 135;
            Angle[2]    = 45;

            printf("Now LegNum:%d\n",legNum);
            break;
    }

    printf("Angle:%5.2f -- %5.2f -- %5.2f\n",Angle[0],Angle[1],Angle[2]);

    switch(legNum){
        case 1: FK_LUMove(Angle[0],Angle[1],Angle[2],0);  break;   //1--->左前
        case 2: FK_LBMove(Angle[0],Angle[1],Angle[2],0);  break;   //2--->左后
        case 3: FK_RBMove(Angle[0],Angle[1],Angle[2],0);  break;   //3--->右后
        case 4: FK_RUMove(Angle[0],Angle[1],Angle[2],0);  break;   //4--->右前
    }

    return legNum;
}

// ********************************** //
// ************** 逆解 ************** //
// ********************************** //
void IK_LUMove(float x,float y,float z,uint16_t Time){

    float angle[3] = {};

//    IK_LBPoint[0] = x;
//    IK_LBPoint[1] = y;
//    IK_LBPoint[2] = z;

    IK(x,y,z,angle);                    // 逆解

    angle[0] = 90.0 + angle[0];
    angle[1] = 90.0 + angle[1];
    angle[2] = 00.0 + angle[1] - angle[2];

    FK_LUMove(angle[0],angle[1],angle[2],Time);
}

void IK_LBMove(float x,float y,float z,uint16_t Time){

    float angle[3] = {};

//    IK_LBPoint[0] = x;
//    IK_LBPoint[1] = y;
//    IK_LBPoint[2] = z;

    IK(x,y,z,angle);                    // 逆解

    angle[0] = 90.0 + angle[0];
    angle[1] = 90.0 + angle[1];
    angle[2] = 00.0 + angle[1] - angle[2];

    FK_LBMove(angle[0],angle[1],angle[2],Time);
}

void IK_RUMove(float x,float y,float z,uint16_t Time){

    float angle[3] = {};

//    IK_LBPoint[0] = x;
//    IK_LBPoint[1] = y;
//    IK_LBPoint[2] = z;

    IK(x,y,z,angle);                    // 逆解

    angle[0] = 90.0 + angle[0];
    angle[1] = 90.0 + angle[1];
    angle[2] = 00.0 + angle[1] - angle[2];

    FK_RUMove(angle[0],angle[1],angle[2],Time);
}

void IK_RBMove(float x,float y,float z,uint16_t Time){

    float angle[3] = {};

//    IK_LBPoint[0] = x;
//    IK_LBPoint[1] = y;
//    IK_LBPoint[2] = z;

    IK(x,y,z,angle);                    // 逆解

    angle[0] = 90.0 + angle[0];
    angle[1] = 90.0 + angle[1];
    angle[2] = 00.0 + angle[1] - angle[2];

    FK_RBMove(angle[0],angle[1],angle[2],Time);
}

void IK_LegMove(float Point[],uint8_t LEGNum,uint16_t Time,uint8_t Print){
    float angle[3] = {};

    IK(Point[0],Point[1],Point[2],angle);

    angle[0] = 90.0 + angle[0];
    angle[1] = 90.0 + angle[1];
    angle[2] = 00.0 + angle[1] - angle[2];

    if(Print){
        printf("Angle:%5.2f -- %5.2f -- %5.2f\n",angle[0],angle[1],angle[2]);
    }

    switch(LEGNum){
        case 1: FK_LUMove(angle[0],angle[1],angle[2],Time);  break;   //1--->左前
        case 2: FK_LBMove(angle[0],angle[1],angle[2],Time);  break;   //2--->左后
        case 3: FK_RBMove(angle[0],angle[1],angle[2],Time);  break;   //3--->右后
        case 4: FK_RUMove(angle[0],angle[1],angle[2],Time);  break;   //4--->右前
    }
}

void LegPointDebug(float point[],uint8_t cmd,uint8_t PrintPoint,uint8_t PrintAngle){

    uint8_t offset     = 10;
    uint8_t legNum     = 1;

    switch(cmd){
        //X
        case '4': point[0] += offset;break;
        case '1': point[0] -= offset;break;
        //Y
        case '5': point[1] += offset;break;
        case '2': point[1] -= offset;break;
        //Z
        case '6': point[2] += offset;break;
        case '3': point[2] -= offset;break;
        //Leg
        case '0':
            legNum++;
            if(legNum == 5){
                legNum = 1;
            }
            printf("LegNum:%d\n",legNum);
            break;
    }

    if(PrintPoint)
        printf("Point:%5.3f -- %5.3f -- %5.3f\n",point[0],point[1],point[2]);

    IK_LegMove(point,legNum,0,PrintAngle);
}

/* ------------------ 姿态控制 ------------------ */
// 姿态坐标位置
void PosToPoint(float x,float y,float z,uint16_t Time){
    IK_RUMove(XS+x, YS+y, ZS+z,Time);
    IK_LUMove(XS+x, YS-y, ZS+z,Time);
    IK_RBMove(XS+x, YS+y, ZS+z,Time);
    IK_LBMove(XS+x, YS-y, ZS+z,Time);
}


/* ------------------ 步态控制 ------------------ */
/**
 * @brief Trot步态
 *
 */
void Trot(uint8_t step,uint8_t dir){

    uint8_t DSD = 0;   // 延时

    float sigma,xep_b,xep_z,zep;
    float t = 0;

    float speed = 0.08;                         // 步频

    float Ts        = 1;
    float fai       = 0.5;                      // 周期T， 占空比fai(支撑相)
    float offset    = 10;                       // 步长

    float xs = XS - offset, xf = XS + offset;   // x起始坐标，终点坐标
    float zs = ZS,          zh = 10;            // z起始坐标，抬腿高度

    //足端坐标
    float x1 = XS, x2 = XS, x3 = XS, x4 = XS;
    float y1 = YS, y2 = YS, y3 = YS, y4 = YS;
    float z1 = ZS, z2 = ZS, z3 = ZS, z4 = ZS;

    printf("Trot Ready...");

    // 单个步态周期
    for(uint8_t i = 0;i < step;i ++){
        while(t < 1){
            // 前半周期
            if(t <= Ts*fai){
                sigma  = 2*PI*t/(fai*Ts);
                zep    = -zh*(1-cos(sigma))/2+zs;                    // z轴坐标

                xep_b  = (xf - xs)*(sigma-sin(sigma))/(2*PI) + xs;  // 摆动相x轴坐标
                xep_z  = (xs - xf)*(sigma-sin(sigma))/(2*PI) + xf;  // 支撑相x轴坐标

                if(dir != 2){
                    z1 = zep;
                    z2 = zs;
                    z3 = zep;
                    z4 = zs;
                }

                if(dir == 1){
                    x1 = xep_b;
                    x2 = xep_z;
                    x3 = xep_b;
                    x4 = xep_z;
                }else if(dir == 0){
                    x1 = xep_z;
                    x2 = xep_b;
                    x3 = xep_z;
                    x4 = xep_b;
                }else if(dir == 2){
                    x1 = XS;
                    x2 = XS;
                    x3 = XS;
                    x4 = XS;
                }
            }

            // 后半周期
            if(Ts*fai < t && t < Ts){
                sigma  = 2*PI*(t - Ts*fai)/(fai*Ts);
                zep    = -zh*(1-cos(sigma))/2 + zs;                  // z轴坐标

                xep_b  = (xf - xs)*(sigma-sin(sigma))/(2*PI) + xs;  // 摆动相x轴坐标
                xep_z  = (xs - xf)*(sigma-sin(sigma))/(2*PI) + xf;  // 支撑相x轴坐标

                if(dir != 2){
                    z1 = zs;
                    z2 = zep;
                    z3 = zs;
                    z4 = zep;
                }

                if(dir == 1){
                    x1 = xep_z;
                    x2 = xep_b;
                    x3 = xep_z;
                    x4 = xep_b;
                }else if(dir == 0){
                    x1 = xep_b;
                    x2 = xep_z;
                    x3 = xep_b;
                    x4 = xep_z;
                }else if(dir == 2){
                    x1 = XS;
                    x2 = XS;
                    x3 = XS;
                    x4 = XS;
                }
            }

            t = t + speed;

            IK_RUMove(x1,y1,z1,DSD);
            IK_LUMove(x2,y2,z2,DSD);

            IK_LBMove(x3,y3,z3,DSD);
            IK_RBMove(x4,y4,z4,DSD);
        }
        t = 0;
    }

    Robot_IK_ResetPose();
}

/**
 * @brief TrotRL步态
 *
 */
void TrotRL(uint8_t step,uint8_t dir){

    uint8_t DSD = 0;   // 延时

    float sigma,yep_b,yep_z,zep;
    float t = 0;

    float speed = 0.08;                         // 步频

    float Ts        = 1;
    float fai       = 0.5;                      // 周期T， 占空比fai(支撑相)
    float offset    = 10;                       // 步长

    float ys = YS - offset, yf = YS + offset;   // x起始坐标，终点坐标
    float zs = ZS,          zh = 10;            // z起始坐标，抬腿高度

    //足端坐标
    float x1 = XS, x2 = XS, x3 = XS, x4 = XS;
    float y1 = YS, y2 = YS, y3 = YS, y4 = YS;
    float z1 = ZS, z2 = ZS, z3 = ZS, z4 = ZS;

    printf("TrotRL Ready...");

    // 单个步态周期
    for(uint8_t i = 0;i < step;i ++){
        while(t < 1){
            // 前半周期
            if(t <= Ts*fai){
                sigma  = 2*PI*t/(fai*Ts);
                zep    = -zh*(1-cos(sigma))/2+zs;                    // z轴坐标

                yep_b  = (yf - ys)*(sigma-sin(sigma))/(2*PI) + ys;  // 摆动相y轴坐标
                yep_z  = (ys - yf)*(sigma-sin(sigma))/(2*PI) + yf;  // 支撑相y轴坐标

                if(dir != 2){
                    z1 = zep;
                    z2 = zs;
                    z3 = zep;
                    z4 = zs;
                }

                if(dir == 1){
                    y1 = yep_z;
                    y2 = yep_z;
                    y3 = yep_b;
                    y4 = yep_b;
                }else if(dir == 0){
                    y1 = yep_b;
                    y2 = yep_b;
                    y3 = yep_z;
                    y4 = yep_z;
                }else if(dir == 2){
                    y1 = YS;
                    y2 = YS;
                    y3 = YS;
                    y4 = YS;
                }
            }

            // 后半周期
            if(Ts*fai < t && t < Ts){
                sigma  = 2*PI*(t - Ts*fai)/(fai*Ts);
                zep    = -zh*(1-cos(sigma))/2 + zs;                  // z轴坐标

                yep_b  = (yf - ys)*(sigma-sin(sigma))/(2*PI) + ys;  // 摆动相y轴坐标
                yep_z  = (ys - yf)*(sigma-sin(sigma))/(2*PI) + yf;  // 支撑相y轴坐标

                if(dir != 2){
                    z1 = zs;
                    z2 = zep;
                    z3 = zs;
                    z4 = zep;
                }

                if(dir == 1){
                    y1 = yep_b;
                    y2 = yep_b;
                    y3 = yep_z;
                    y4 = yep_z;
                }else if(dir == 0){
                    y1 = yep_z;
                    y2 = yep_z;
                    y3 = yep_b;
                    y4 = yep_b;
                }else if(dir == 2){
                    y1 = YS;
                    y2 = YS;
                    y3 = YS;
                    y4 = YS;
                }
            }

            t = t + speed;

            IK_RUMove(x1,y1,z1,DSD);
            IK_LUMove(x2,y2,z2,DSD);

            IK_LBMove(x3,y3,z3,DSD);
            IK_RBMove(x4,y4,z4,DSD);
        }
        t = 0;
    }

    Robot_IK_ResetPose();
}

/**
 * @brief TrotTurn步态
 *
 */
void TrotTurn(uint8_t step,uint8_t dir){

    uint8_t DSD = 0;   // 延时

    float sigma,yep_b,yep_z,zep;
    float t = 0;

    float speed = 0.08;                         // 步频

    float Ts        = 1;
    float fai       = 0.5;                      // 周期T， 占空比fai(支撑相)
    float offset    = 10;                       // 步长

    float ys = YS - offset, yf = YS + offset;   // x起始坐标，终点坐标
    float zs = ZS,          zh = 10;            // z起始坐标，抬腿高度

    //足端坐标
    float x1 = XS, x2 = XS, x3 = XS, x4 = XS;
    float y1 = YS, y2 = YS, y3 = YS, y4 = YS;
    float z1 = ZS, z2 = ZS, z3 = ZS, z4 = ZS;

    printf("TrotRL Ready...");

    // 单个步态周期
    for(uint8_t i = 0;i < step;i ++){
        while(t < 1){
            // 前半周期
            if(t <= Ts*fai){
                sigma  = 2*PI*t/(fai*Ts);
                zep    = -zh*(1-cos(sigma))/2+zs;                    // z轴坐标

                yep_b  = (yf - ys)*(sigma-sin(sigma))/(2*PI) + ys;  // 摆动相y轴坐标
                yep_z  = (ys - yf)*(sigma-sin(sigma))/(2*PI) + yf;  // 支撑相y轴坐标

                if(dir != 2){
                    z1 = zep;
                    z2 = zs;
                    z3 = zep;
                    z4 = zs;
                }

                if(dir == 1){
                    y1 = yep_z;
                    y2 = yep_z;
                    y3 = yep_z;
                    y4 = yep_z;
                }else if(dir == 0){
                    y1 = yep_b;
                    y2 = yep_b;
                    y3 = yep_b;
                    y4 = yep_b;
                }else if(dir == 2){
                    y1 = YS;
                    y2 = YS;
                    y3 = YS;
                    y4 = YS;
                }
            }

            // 后半周期
            if(Ts*fai < t && t < Ts){
                sigma  = 2*PI*(t - Ts*fai)/(fai*Ts);
                zep    = -zh*(1-cos(sigma))/2 + zs;                  // z轴坐标

                yep_b  = (yf - ys)*(sigma-sin(sigma))/(2*PI) + ys;  // 摆动相y轴坐标
                yep_z  = (ys - yf)*(sigma-sin(sigma))/(2*PI) + yf;  // 支撑相y轴坐标

                if(dir != 2){
                    z1 = zs;
                    z2 = zep;
                    z3 = zs;
                    z4 = zep;
                }

                if(dir == 1){
                    y1 = yep_b;
                    y2 = yep_b;
                    y3 = yep_b;
                    y4 = yep_b;
                }else if(dir == 0){
                    y1 = yep_z;
                    y2 = yep_z;
                    y3 = yep_z;
                    y4 = yep_z;
                }else if(dir == 2){
                    y1 = YS;
                    y2 = YS;
                    y3 = YS;
                    y4 = YS;
                }
            }

            t = t + speed;

            IK_RUMove(x1,y1,z1,DSD);
            IK_LUMove(x2,y2,z2,DSD);

            IK_LBMove(x3,y3,z3,DSD);
            IK_RBMove(x4,y4,z4,DSD);
        }
        t = 0;
    }

    Robot_IK_ResetPose();
}